#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
#include <WiFi.h>
#include <esp_now.h>
#include <SimpleFOC.h>
#include <MAC_address.h>
// #define DEBUG
#define LED_pin 2
// 绑定数据接收端
esp_now_peer_info_t peerInfo;
double init_p_sensor = 0;
double init_p_sensor1 = 0;
double motor1_angle = 0, motor2_angle = 0;
// 设置数据结构体
typedef struct struct_message
{
    String board_name = "";
    double motor1_angle = 0;
    double motor2_angle = 0;
} struct_message;
struct_message myFOC;
uint8_t broadcastAddress[6]; // 这里只是创建了一个空数组，我用的是自己写的一个头文件<MAC_address.h>，这个文件里面存储的是所有的板子的地址

MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
TwoWire I2Cone = TwoWire(0);
TwoWire I2Ctwo = TwoWire(1);
//电机参数
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(32, 14, 25, 12); // M1电机

BLDCMotor motor1 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(33, 26, 27, 13); // M2电机

//命令设置
float target_angle = 0;
// Commander command = Commander(Serial);
// void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }

void task_LED(void *pvParam) // 宣告任务LED闪烁
{
    /* 使用ledc通道1，设置频率为1kHz，分辨率为10位 */
    int freq = 1000;                      // 频率
    int channel = 0;                      // 通道
    int resolution = 8;                   // 分辨率
    ledcSetup(channel, freq, resolution); // 设置通道
    ledcAttachPin(LED_pin, channel);      // 将通道与对应的引脚连接
    while (1)
    {
        // 逐渐变亮
        for (int dutyCycle = 0; dutyCycle <= 255; dutyCycle = dutyCycle + 5)
        {
            ledcWrite(channel, dutyCycle); // 输出PWM
            vTaskDelay(pdMS_TO_TICKS(20));
        }
        // 逐渐变暗
        for (int dutyCycle = 255; dutyCycle >= 0; dutyCycle = dutyCycle - 5)
        {
            ledcWrite(channel, dutyCycle); // 输出PWM
            vTaskDelay(pdMS_TO_TICKS(20));
        }
    }
}
// 数据接收回调函数
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len)
{
    memcpy(&myFOC, incomingData, sizeof(myFOC));
    if (myFOC.board_name == "FOC1")
    {
        motor1_angle = myFOC.motor1_angle;
        motor2_angle = myFOC.motor2_angle;
    }
    // esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&myFOC, sizeof(myFOC));
    // Serial.printf("电机一：%lf,电机二：%lf\n", motor1_angle, motor2_angle);
#ifdef DEBUG
    Serial.print("board_name: ");
    Serial.println(myFOC.board_name);
    Serial.print("motor1_angle: ");
    Serial.println(myFOC.motor1_angle);
    Serial.print("motor2_angle:");
    Serial.println(myFOC.motor2_angle);
    Serial.println();
#endif
}
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status)
{
#ifdef CALLBACK
    char macStr[18];
    Serial.print("Packet to: ");
    snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
             mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
    Serial.println(macStr);
    Serial.print("Send status: ");
    Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
    Serial.println();
#endif
}
void espNowInit()
{
    //将字符数组中存储的MAC地址转化为16进制数组中存储的数据,这样通过修改编号就可以决定与那个板子进行通讯
    mac_str_to_bin(address[0], broadcastAddress);
    // Init ESP-NOW
    WiFi.mode(WIFI_STA);
    Serial.println(WiFi.macAddress());
    if (esp_now_init() != ESP_OK)
    {
        Serial.println("Error initializing ESP-NOW");
        return;
    }
    // 设置接收数据回调函数
    esp_now_register_recv_cb(OnDataRecv);
    esp_now_register_send_cb(OnDataSent);
    peerInfo.channel = 0;
    peerInfo.encrypt = false;

    // Add peer
    memcpy(peerInfo.peer_addr, broadcastAddress, 6);
    // 检查设备是否配对成功
    if (esp_now_add_peer(&peerInfo) != ESP_OK)
    {
        Serial.println("Failed to add peer1");
        return;
    }
}
void setup()
{
    Serial.begin(115200);
    xTaskCreate(task_LED, "task_LED", 1000, NULL, 1, NULL);
    I2Cone.begin(5, 18, 400000);  // CN1 磁编码器
    I2Ctwo.begin(21, 22, 400000); // CN2 磁编码器
    sensor.init(&I2Cone);
    sensor1.init(&I2Ctwo);
    //连接motor对象与传感器对象
    motor.linkSensor(&sensor);
    motor1.linkSensor(&sensor1);

    //供电电压设置 [V]
    driver.voltage_power_supply = 24;
    driver.init();

    driver1.voltage_power_supply = 24;
    driver1.init();
    //连接电机和driver对象
    motor.linkDriver(&driver);
    motor1.linkDriver(&driver1);

    // FOC模型选择
    motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
    motor1.foc_modulation = FOCModulationType::SpaceVectorPWM;
    motor.torque_controller = TorqueControlType::voltage;
    motor1.torque_controller = TorqueControlType::voltage;
    //运动控制模式设置
    motor.controller = MotionControlType::angle;
    motor1.controller = MotionControlType::angle;

    //速度PI环设置
    motor.PID_velocity.P = 0.7;
    motor.PID_velocity.I = 0.5;
    motor.PID_velocity.D = 0.002;
    motor.PID_velocity.output_ramp = 900.0;
    // Low pass filtering time constant 
    //速度低通滤波时间常数
    motor.LPF_velocity.Tf = 0.2;
    motor.PID_velocity.limit = 40.0;

    //速度PI环设置
    motor1.PID_velocity.P = 0.7;
    motor1.PID_velocity.I = 0.5;
    motor1.PID_velocity.D = 0.002;
    motor1.PID_velocity.output_ramp = 900.0;
    // Low pass filtering time constant 
    //速度低通滤波时间常数
    motor1.LPF_velocity.Tf = 0.2;
    motor1.PID_velocity.limit = 40.0;

    //角度P环设置
    motor.P_angle.P = 15.0;
    motor1.P_angle.P = 15.0;
    //最大电机限制电机
    motor.voltage_limit = 24;
    motor1.voltage_limit = 24;

    motor.useMonitoring(Serial);
    motor1.useMonitoring(Serial);

    //初始化电机
    motor.init();
    motor1.init();
    //初始化 FOC
    motor.initFOC();
    // motor1.initFOC(2.3838, Direction::CCW);
    motor1.initFOC();
    // motor1.initFOC();
    Serial.println("Motor ready.");
    init_p_sensor = sensor.getAngle();
    init_p_sensor1 = sensor1.getAngle();
    Serial.println(init_p_sensor);
    Serial.println(init_p_sensor1);

    // command.add('T', doTarget, "target angle");

    Serial.println(F("Motor ready."));
    Serial.println(F("Set the target angle using serial terminal:"));
    // 初始化 ESP-NOW
    espNowInit();
}
void loop()
{
    motor.loopFOC();
    motor1.loopFOC();
    motor.move(motor1_angle);
    motor1.move(motor2_angle);
    // Serial.printf("电机一：%lf,电机二：%lf\n", motor1_angle, motor2_angle);
}
// 同样，我们在开头引入了 ESP-NOW 相关的头文件：WiFi.h 与 esp_now.h。然后定义了传感器的引脚和一个名为 myFOC 的结构体，该结构体由 3 个不同数据组成，分别是开发板名称（board_name），光线值（light）与声音值（sound），与数据发送端定义的一模一样。
// 我们注册了一个 OnDataRecv() 的数据接收回调函数，该函数反馈了 ESP-NOW 数据的接收状态，当接收到来自其他开发板的消息时，将消息保存到结构体 myFOC 中，并在串口中打印出接收到的消息。
// 这里 ESP-NOW 的消息的接收属于无阻塞的接收方式，不受延时函数 delay() 的影响，这意味着 loop() 里面可以执行其他任务而不会影响到板间的通讯，同一块板子既可以当做接收方亦可以当做发送方互不影响。这里可以同时接收来自多块板子的信息，但前提是保持相同的结构体，当然你也可以不用结构体用普通的单一类型的数据类型比如字符串之类的，这里用结构体的原因是需要发送的数据类型有多个，使用结构体相对灵活。
